#include "srpq_controller/AdvanceControllers/SportsController/SportsController.h"
using namespace std;

template class SportsController<double>;
template class SportsController<float>;

template <typename T>
SportsController<T>::SportsController(ControlFSMData<T>* _controlFSMData) {
    _data = _controlFSMData;

    _ini_yaw = this->_data->_stateEstimator->getResult().rpy[2];
    //  LF
    _dogModel[0].OQ_init << length / 2.0, width / 2.0, 0.0;
    _dogModel[0].OP_init << length / 2.0, width / 2.0, _ini_height;
    // RF
    _dogModel[1].OQ_init << length / 2.0, -width / 2.0, 0.0;
    _dogModel[1].OP_init << length / 2.0, -width / 2.0, _ini_height;
    // LB
    _dogModel[2].OQ_init << -length / 2.0, width / 2.0, 0.0;
    _dogModel[2].OP_init << -length / 2.0, width / 2.0, _ini_height;
    // RB
    _dogModel[3].OQ_init << -length / 2.0, -width / 2.0, 0.0;
    _dogModel[3].OP_init << -length / 2.0, -width / 2.0, _ini_height;
}

template <typename T>
void SportsController<T>::_SetupCommand(void) {
    auto& seResult = _data->_stateEstimator->getResult();

    // set height
    _ini_height = this->_data->_desiredStateCommand->rcCommand->height_variation;

    // 判断跳跃是否触发
    if (jump_state.trigger_pressed(iter, _data->_desiredStateCommand->trigger_pressed)) {
        Vec3<T> jump_ready(0, 0, -0.1);
        
        switch ((int)this->_data->_desiredStateCommand->rcCommand->variable[2])
        {   
            // JUMP
            case 0:
                front_jump_begin << 0, 0, -0.15;
                front_jump_end << 0, 0, -0.36;
                break;
            
            // FRONT JUMP  LOW
            case 1:
                jump_ready << 0, 0, -0.18;
                calculateJumpPos(0, 0.14, 0.13, jump_ready, &front_jump_begin, &front_jump_end); //  _data->_legController->datas[0].p 
                break;

            // FRONT JUMP  HIGH
            case 2:
                calculateJumpPos(0.001, 0.21, 0.21, jump_ready, &front_jump_begin, &front_jump_end); 
                break;  
            default:
                break;
        }
    }

    // 判断是否需要跳跃
    if (jump_state.should_jump(iter)) {
        switch ((int)this->_data->_desiredStateCommand->rcCommand->variable[2])
        {   
            case 0:
                this->_data->_gaitScheduler->gaitData._nextGait = GaitType::JUMP;
                break;
            case 1:
                this->_data->_gaitScheduler->gaitData._nextGait = GaitType::FRONT_JUMP_L;
                break;
            case 2:
                this->_data->_gaitScheduler->gaitData._nextGait = GaitType::FRONT_JUMP_H;
                break;  
            default:
                break;
        }
        
        currently_jumping = true;
    } else {
        currently_jumping = false;
    }

    // choose gait
    if (!currently_jumping) {
        switch ((int)this->_data->_desiredStateCommand->rcCommand->variable[0]) {
            case 0:
                this->_data->_gaitScheduler->gaitData._nextGait = GaitType::TROT;
                break;
            case 3:
                this->_data->_gaitScheduler->gaitData._nextGait = GaitType::STATIC_WALK;
                break;
            case 4:
                this->_data->_gaitScheduler->gaitData._nextGait = GaitType::STAND;
            default:
                break;
        }
    }
}

template <typename T>
void SportsController<T>::run(void) {
    // convert commands
    _SetupCommand();

    // gait schedule
    switch ((int)this->_data->_gaitScheduler->gaitData._currentGait) {
        case (int)GaitType::STAND:
            Stand();
            break;
        case (int)GaitType::TROT:
            // Trot(0.07, 0.015);
            Trot(0.05, 0);
            break;
        case (int)GaitType::JUMP:
            Jump();
            
            break;
        case (int)GaitType::STATIC_WALK:
            Walk(0.1, 0.0);
            break;
        case (int)GaitType::FRONT_JUMP_L:
            Front_Jump_Low();
            break;
        case (int)GaitType::FRONT_JUMP_H:
            Front_Jump_High();
            break;               
        default:
            break;
    }

    // imu fix
    // if (1) {
    //     auto seResult = this->_data->_stateEstimator->getResult();
    //     limitRpy(seResult.rpy, 0.38, 0.0, 0.1);
    //     calculatePosBias_rad(seResult.rpy[0], seResult.rpy[1], 0);
    //     for (int leg = 0; leg < 4; leg++) {
    //         this->_data->_legController->commands[leg].pDes[0] += _dogModel[leg].QP_Q[0];
    //         // this->_data->_legController->commands[leg].pDes[1] += _dogModel[leg].QP_Q[1];
    //         this->_data->_legController->commands[leg].pDes[2] += (_dogModel[leg].QP_Q[2] - _ini_height);
    //     }
    // }


    for (int leg = 0; leg < 4; leg++) {
        // x bias
        if(this->_data->_gaitScheduler->gaitData._currentGait == GaitType::TROT ||   this->_data->_gaitScheduler->gaitData._currentGait == GaitType::STAND){
            this->_data->_legController->commands[leg].pDes[0] -= _ini_xbias;
        }
        
        Vec3<T> qDes;
        computeLegAngle(*this->_data->_quadruped, this->_data->_legController->commands[leg].pDes, &qDes);
        this->_data->_legController->commands[leg].qDes = qDes;
        this->_data->_legController->commands[leg].qdDes =
            this->_data->_legController->datas[leg].J.inverse() * this->_data->_legController->commands[leg].vDes;

        // q bias
        switch ((int)this->_data->_gaitScheduler->gaitData._currentGait) {
            case (int)GaitType::TROT:
                if (leg == 0 || leg == 1) {
                    this->_data->_legController->commands[leg].qDes[0] -= 0.018 * 3;
                } else if (leg == 2 || leg == 3) {
                    this->_data->_legController->commands[leg].qDes[0] += 0.018 * 1.5;
                }
                break;
            case (int)GaitType::JUMP:
                // this->_data->_legController->commands[leg].qDes[0] += 0.018 * 4;
                break;
           case (int)GaitType::FRONT_JUMP_L:
                // this->_data->_legController->commands[leg].qDes[0] += 0.018 * 4;
                break;
            case (int)GaitType::FRONT_JUMP_H:
                // this->_data->_legController->commands[leg].qDes[0] += 0.018 * 4;
                break;
            default:
                break;
        }         

        // clear tau
        this->_data->_legController->commands[leg].tauFeedForward << 0, 0, 0;
        this->_data->_legController->commands[leg].forceFeedForward << 0, 0, 0;
    }

    // _data->_desiredStateCommand->trigger_pressed = false;
    iter++;
    // if(iter == 1500){
    //     _data->_desiredStateCommand->trigger_pressed = true;
    //     // iter = 0;
    // }
    // else{
    //     _data->_desiredStateCommand->trigger_pressed = false;
    // }
}

// STAND
template <typename T>
void SportsController<T>::Stand(void) {
    for (int leg = 0; leg < 4; leg++) {
        this->_data->_legController->commands[leg].pDes << 0.0, 0.0, _ini_height;
        // this->_data->_legController->commands[leg].kpJoint << 120, 0, 0, 0, 85, 0, 0, 0, 85;
        this->_data->_legController->commands[leg].kpJoint << 120, 0, 0, 0, 40, 0, 0, 0, 40;
        this->_data->_legController->commands[leg].kdJoint << 1.5, 0, 0, 0, 1.5, 0, 0, 0, 1.5;
    }
}

// TROT
template <typename T>
void SportsController<T>::Trot(T upAmp, T downAmp) {
    const rc_control_settings* rc_cmd = this->_data->_desiredStateCommand->rcCommand;
    Vec3<T> vel_bias(0.0, 0, 0.0);
    Vec3<T> stepLength[4];

    // calculate step length according to v_des and bias
    for (int leg = 0; leg < 4; leg++) {
        stepLength[leg] << (rc_cmd->v_des[0] + vel_bias(0) ) * 0.45 * 0.5,
            (rc_cmd->v_des[1] + vel_bias(1)) * 0.45  +
                ((leg == 0 || leg == 1) ? 1. : -1.) * (rc_cmd->omega_des[2] + vel_bias(2)) * 0.45 * 0.5,
            0;
    }

    Vec3<T> initialPosition;
    Vec3<T> finalPosition;
    for (int leg = 0; leg < 4; leg++) {
        // target position
        initialPosition << -stepLength[leg](0) / 2.0, -stepLength[leg](1) / 2.0, _ini_height;
        finalPosition << stepLength[leg](0) / 2.0, stepLength[leg](1) / 2.0, _ini_height;

        //支撑相
        if (this->_data->_gaitScheduler->gaitData.contactStateScheduled(leg) == 1) {
            _footSwingTrajectory.setHeight(-downAmp);
            _footSwingTrajectory.setInitialPosition(finalPosition);
            _footSwingTrajectory.setFinalPosition(initialPosition);
            _footSwingTrajectory.computeSwingTrajectoryBezier(
                this->_data->_gaitScheduler->gaitData.phaseStance(leg),
                this->_data->_gaitScheduler->gaitData.periodTimeNominal *
                    this->_data->_gaitScheduler->gaitData.switchingPhaseNominal);
            this->_data->_legController->commands[leg].kpJoint << 150, 0, 0, 0, 60, 0, 0, 0, 60;
            this->_data->_legController->commands[leg].kdJoint << 2., 0, 0, 0, 1.8, 0, 0, 0, 1.8;
            // this->_data->_legController->commands[leg].kpJoint << 150, 0, 0, 0, 100, 0, 0, 0, 100;
            // this->_data->_legController->commands[leg].kdJoint << 2.0, 0, 0, 0, 2.0, 0, 0, 0, 2.0;
        }

        //摆动相
        else {
            _footSwingTrajectory.setHeight(upAmp);
            _footSwingTrajectory.setInitialPosition(initialPosition);
            _footSwingTrajectory.setFinalPosition(finalPosition);
            _footSwingTrajectory.computeSwingTrajectoryBezier(
                this->_data->_gaitScheduler->gaitData.phaseSwing(leg),
                this->_data->_gaitScheduler->gaitData.periodTimeNominal *
                    (1.0 - this->_data->_gaitScheduler->gaitData.switchingPhaseNominal));
            // this->_data->_legController->commands[leg].kpJoint << 150, 0, 0, 0, 85, 0, 0, 0, 85;
            // this->_data->_legController->commands[leg].kdJoint << 2., 0, 0, 0, 1.8, 0, 0, 0, 1.8;
            this->_data->_legController->commands[leg].kpJoint << 150, 0, 0, 0, 60, 0, 0, 0, 60;
            this->_data->_legController->commands[leg].kdJoint << 2., 0, 0, 0, 1.8, 0, 0, 0, 1.8;
        }
        this->_data->_legController->commands[leg].pDes = _footSwingTrajectory.getPosition();
        this->_data->_legController->commands[leg].vDes = _footSwingTrajectory.getVelocity();
    }
}

template <typename T>
void SportsController<T>::Walk(T upAmp, T downAmp) {
    const rc_control_settings* rc_cmd = this->_data->_desiredStateCommand->rcCommand;
    Vec3<T> vel_bias(0.25, 0, 0.04);
    // Vec3<T> vel_bias(0.25, 0, 0);
    Vec3<T> stepLength[4];

    // calculate step length according to v_des and bias
    for (int leg = 0; leg < 4; leg++) {
        stepLength[leg] << (rc_cmd->v_des[0] + vel_bias(0)) * 0.45 * 0.5,
            (rc_cmd->v_des[1] + vel_bias(1)) * 0.45 * 0.5 +
                ((leg == 0 || leg == 1) ? 1. : -1.) * (rc_cmd->omega_des[2] + vel_bias(2)) * 0.45 * 0.5,
            0;
    }

    Vec3<T> initialPosition;
    Vec3<T> finalPosition;

    T phaseStance = 0;
    for (int leg = 0; leg < 4; leg++) {
        if (this->_data->_gaitScheduler->gaitData.phaseSwing(leg) > 0) {
            phaseStance = this->_data->_gaitScheduler->gaitData.phaseSwing(leg);
            break;
        }
    }
    for (int leg = 0; leg < 4; leg++) {
        // target position
        initialPosition << -stepLength[leg](0) / 2.0, -stepLength[leg](1) / 2.0, _ini_height;
        finalPosition << stepLength[leg](0) / 2.0, stepLength[leg](1) / 2.0, _ini_height;

        //支撑相
        if (this->_data->_gaitScheduler->gaitData.contactStateScheduled(leg) == 1) {
            _footSwingTrajectory.setHeight(-downAmp);
            _footSwingTrajectory.setInitialPosition(finalPosition);
            _footSwingTrajectory.setFinalPosition(initialPosition);
            _footSwingTrajectory.computeSwingTrajectoryBezier(
                phaseStance, this->_data->_gaitScheduler->gaitData.periodTimeNominal *
                                 (1.0 - this->_data->_gaitScheduler->gaitData.switchingPhaseNominal));
            this->_data->_legController->commands[leg].kpJoint << 150, 0, 0, 0, 70, 0, 0, 0, 70;
            this->_data->_legController->commands[leg].kdJoint << 2., 0, 0, 0, 1.8, 0, 0, 0, 1.8;
            // this->_data->_legController->commands[leg].kpJoint << 150, 0, 0, 0, 100, 0, 0, 0, 100;
            // this->_data->_legController->commands[leg].kdJoint << 2.0, 0, 0, 0, 2.0, 0, 0, 0, 2.0;
        }

        //摆动相
        else {
            _footSwingTrajectory.setHeight(upAmp);
            _footSwingTrajectory.setInitialPosition(initialPosition);
            _footSwingTrajectory.setFinalPosition(finalPosition);
            _footSwingTrajectory.computeSwingTrajectoryBezier(
                this->_data->_gaitScheduler->gaitData.phaseSwing(leg),
                this->_data->_gaitScheduler->gaitData.periodTimeNominal *
                    (1.0 - this->_data->_gaitScheduler->gaitData.switchingPhaseNominal));
            this->_data->_legController->commands[leg].kpJoint << 150, 0, 0, 0, 70, 0, 0, 0, 70;
            this->_data->_legController->commands[leg].kdJoint << 2., 0, 0, 0, 3., 0, 0, 0, 3.;
            // this->_data->_legController->commands[leg].kpJoint << 150, 0, 0, 0, 60, 0, 0, 0, 60;
            // this->_data->_legController->commands[leg].kdJoint << 2., 0, 0, 0, 1.8, 0, 0, 0, 1.8;
        }
        this->_data->_legController->commands[leg].pDes = _footSwingTrajectory.getPosition();
        this->_data->_legController->commands[leg].vDes = _footSwingTrajectory.getVelocity();
    }
}

// JUMP
template <typename T>
void SportsController<T>::Jump(void) {
    int READY_COUNT = 300;
    int JUMP_COUNT = 60;
    int JUMP_PRO_COUNT = 300;
    Vec3<T> ini_pos(0, 0, _ini_height);
    Vec3<T> down_pos(0, 0, -0.15);
    Vec3<T> up_pos(0, 0, -0.36);
    T jump_strength = 60;

    int RE_ITER = jump_state.START_ITER + READY_COUNT + JUMP_COUNT + JUMP_PRO_COUNT;

    for (int leg = 0; leg < 4; leg++) {
        if (iter > jump_state.START_ITER) {
            // // test
            // if(leg == 0 || leg == 1){
            //     down_pos <<  0, 0.06, -0.15;
            //     up_pos << 0, -0.06, -0.31;
            // }
            // else{
            //     down_pos <<  0, -0.06, -0.15;
            //     up_pos << 0, 0.06, -0.31;
            // }
            
            // control param
            this->_data->_legController->commands[leg].kpJoint << 120, 0, 0, 0, 20, 0, 0, 0, 20;
            this->_data->_legController->commands[leg].kdJoint << 1.5, 0, 0, 0, 1.5, 0, 0, 0, 1.5;

            // 缩腿
            if (iter < jump_state.START_ITER + READY_COUNT) {
                this->_data->_legController->commands[leg].pDes = calculateProgressPos(
                    iter, jump_state.START_ITER, jump_state.START_ITER + READY_COUNT, ini_pos, down_pos);
                this->_data->_legController->commands[leg].kpJoint << 120, 0, 0, 0, 85, 0, 0, 0, 85;
                this->_data->_legController->commands[leg].kdJoint << 1.5, 0, 0, 0, 1.5, 0, 0, 0, 1.5;
            }

            // 跳跃
            else if (iter < jump_state.START_ITER + READY_COUNT + JUMP_COUNT) {
                int end_count = jump_state.START_ITER + READY_COUNT + JUMP_COUNT;
                // if (leg == 2 || leg == 3) end_count -= 30;
                this->_data->_legController->commands[leg].pDes =
                    calculateProgressPos(iter, jump_state.START_ITER + READY_COUNT, end_count, down_pos, up_pos);
                this->_data->_legController->commands[leg].kpJoint << 120, 0, 0, 0, jump_strength, 0, 0, 0, jump_strength;
            }

            else if (iter < jump_state.START_ITER + READY_COUNT + JUMP_COUNT + JUMP_PRO_COUNT) {
                this->_data->_legController->commands[leg].pDes = up_pos;
                this->_data->_legController->commands[leg].kpJoint << 120, 0, 0, 0, jump_strength, 0, 0, 0, jump_strength;
            }

            // 维持站立
            else {
                this->_data->_legController->commands[leg].pDes = ini_pos;
            }
        }
        this->_data->_legController->commands[leg].vDes = Vec3<T>::Zero();
    }
}

// Front Jump Low
template <typename T>
void SportsController<T>::Front_Jump_Low(void) {
    int READY_COUNT = 300;
    int JUMP_COUNT = 40;
    int JUMP_PRO_COUNT = 100;
    int RECOVERY_COUNT = 300;
    Vec3<T> ini_pos(0, 0, _ini_height);
    T jump_strength = 65;

    int RE_ITER = jump_state.START_ITER + READY_COUNT + JUMP_COUNT + JUMP_PRO_COUNT;

    for (int leg = 0; leg < 4; leg++) {
        if (iter > jump_state.START_ITER) {
            // control param
            this->_data->_legController->commands[leg].kpJoint << 120, 0, 0, 0, 20, 0, 0, 0, 20;
            this->_data->_legController->commands[leg].kdJoint << 1.5, 0, 0, 0, 1.5, 0, 0, 0, 1.5;

            // 缩腿
            if (iter < jump_state.START_ITER + READY_COUNT) {
                this->_data->_legController->commands[leg].pDes = front_jump_begin;
                // this->_data->_legController->commands[leg].pDes = calculateProgressPos(
                //     iter, jump_state.START_ITER, jump_state.START_ITER + READY_COUNT, ini_pos, front_jump_begin);
                // this->_data->_legController->commands[leg].kpJoint << 120, 0, 0, 0, 85, 0, 0, 0, 85;
                // this->_data->_legController->commands[leg].kdJoint << 1.5, 0, 0, 0, 1.5, 0, 0, 0, 1.5;
            }

            // 跳跃
            else if (iter < jump_state.START_ITER + READY_COUNT + JUMP_COUNT) {
                int end_count = jump_state.START_ITER + READY_COUNT + JUMP_COUNT;
                this->_data->_legController->commands[leg].pDes = calculateProgressPos(
                    iter, jump_state.START_ITER + READY_COUNT, end_count, front_jump_begin, front_jump_end);
                this->_data->_legController->commands[leg].kpJoint << 120, 0, 0, 0, jump_strength, 0, 0, 0, jump_strength;
            }  

            else if (iter < jump_state.START_ITER + READY_COUNT + JUMP_COUNT + JUMP_PRO_COUNT) {
                this->_data->_legController->commands[leg].pDes = front_jump_end;
                this->_data->_legController->commands[leg].kpJoint << 120, 0, 0, 0, jump_strength, 0, 0, 0, jump_strength;
            }

            // 维持站立
            else {
                this->_data->_legController->commands[leg].pDes = ini_pos;
            }
        }
        this->_data->_legController->commands[leg].vDes = Vec3<T>::Zero();
    }
}

// Front Jump High
template <typename T>
void SportsController<T>::Front_Jump_High(void) {
    int READY_COUNT = 300;
    int JUMP_COUNT = 20;
    int JUMP_PRO_COUNT = 100;
    int RECOVERY_COUNT = 300;
    Vec3<T> ini_pos(0, 0, _ini_height);
    Vec3<T> down_pos(0, 0, -0.18);
    Vec3<T> up_pos(0, 0, -0.2);
    T jump_strength = 85;

    int RE_ITER = jump_state.START_ITER + READY_COUNT + JUMP_COUNT + JUMP_PRO_COUNT;

    for (int leg = 0; leg < 4; leg++) {
        if (iter > jump_state.START_ITER) {
            // control param
            this->_data->_legController->commands[leg].kpJoint << 120, 0, 0, 0, 20, 0, 0, 0, 20;
            this->_data->_legController->commands[leg].kdJoint << 1.5, 0, 0, 0, 1.5, 0, 0, 0, 1.5;

            // 缩腿
            if (iter < jump_state.START_ITER + READY_COUNT) {
                this->_data->_legController->commands[leg].pDes = calculateProgressPos(
                    iter, jump_state.START_ITER, jump_state.START_ITER + READY_COUNT, ini_pos, front_jump_begin);
                this->_data->_legController->commands[leg].kpJoint << 120, 0, 0, 0, 85, 0, 0, 0, 85;
                this->_data->_legController->commands[leg].kdJoint << 1.5, 0, 0, 0, 1.5, 0, 0, 0, 1.5;
            }

            // 跳跃
            else if (iter < jump_state.START_ITER + READY_COUNT + JUMP_COUNT) {
                int end_count = jump_state.START_ITER + READY_COUNT + JUMP_COUNT;
                if (leg == 2 || leg == 3) end_count -= 15;
                this->_data->_legController->commands[leg].pDes = calculateProgressPos(
                    iter, jump_state.START_ITER + READY_COUNT, end_count, front_jump_begin, front_jump_end);
                this->_data->_legController->commands[leg].kpJoint << 120, 0, 0, 0, jump_strength, 0, 0, 0, jump_strength;
            }
            else if (iter < jump_state.START_ITER + READY_COUNT + JUMP_COUNT + JUMP_PRO_COUNT) {
                this->_data->_legController->commands[leg].pDes = front_jump_end;
                this->_data->_legController->commands[leg].kpJoint << 120, 0, 0, 0, jump_strength, 0, 0, 0, jump_strength;
            }

            else if (iter < jump_state.START_ITER + READY_COUNT + JUMP_COUNT + JUMP_PRO_COUNT + RECOVERY_COUNT) {
                this->_data->_legController->commands[leg].pDes = down_pos;
                this->_data->_legController->commands[leg].kpJoint << 120, 0, 0, 0, jump_strength, 0, 0, 0, jump_strength;
            }           

            // 维持站立
            else {
                this->_data->_legController->commands[leg].pDes = ini_pos;
            }
        }
        this->_data->_legController->commands[leg].vDes = Vec3<T>::Zero();
    }
}

/**
 * 计算跳的过程
 *
 * @param curr_count 当前的计数值，传入_count
 * @param start_count 开始起跳时的计数值，传入_star_count
 * @param finish_count 跳跃结束后的计数值，传入_finish_count
 * @param ini 起跳前的足端点
 * @param fin 跳跃结束前瞬间的足端点
 * @return pDes 足端目标位置
 */
template <typename T>
Vec3<T> SportsController<T>::calculateProgressPos(const int& curr_count, int start_count, int finish_count,
                                                  const Vec3<T>& ini, const Vec3<T>& fin) {
    T a(0.f);
    T b(1.f);
    int whole_count = finish_count - start_count;
    int count = curr_count - start_count;

    if (start_count <= curr_count && curr_count <= finish_count) {
        b = (T)count / (T)whole_count;
        a = 1.f - b;
    }
    Vec3<T> pDes = a * ini + b * fin;
    return pDes;
}

template <typename T>
void SportsController<T>::calculateJumpPos(T pitch, T height, T length, Vec3<T> ref, Vec3<T>* begin, Vec3<T>* end) {
    // limite
    Vec3<T> min(-0.2, -0.05, -0.4);
    Vec3<T> max(0.2, 0.05, -0.08);
    Vec3<T> temp;
    T pitch_th = 0.054;

    // calculate delta value
    pitch = fminf(fmaxf(pitch, -0.25), 0.25);
    T deltaX = -cos(pitch) * length;
    T deltaZ = - cos(pitch) * height; // sin(pitch) * length 
    if(pitch <= pitch_th) deltaX += sin(pitch) * height;
    else{
        deltaX *= 0.5;
        deltaZ += 1.5 * sin(pitch) * length;
    }

    // begin
    temp = ref;
    if(pitch <= pitch_th){
        temp(0) = sin(pitch) * ref(2);
        temp(2) = cos(pitch) * ref(2);
    }
    *begin = limit(temp, min, max);

    // end
    ref(0) += deltaX;
    ref(2) += deltaZ;
    *end = limit(ref, min, max);
}

template <typename T>
void SportsController<T>::calculatePosBias_rad(T roll, T pitch, T yaw) {
    T SR = sin(roll);
    T CR = cos(roll);
    T SP = sin(pitch);
    T CP = cos(pitch);
    T SY = sin(yaw);
    T CY = cos(yaw);
    Mat3<T> RX, RY, RZ;
    RX << 1, 0, 0, 0, CR, -SR, 0, SR, CR;
    RY << CP, 0, -SP, 0, 1, 0, SP, 0, CP;
    RZ << CY, -SY, 0, SY, CY, 0, 0, 0, 1;
    Mat3<T> Rot = RX * RY * RZ;

    for (int leg = 0; leg < 4; leg++) {
        _dogModel[leg].OQ_now = Rot * _dogModel[leg].OQ_init;
        _dogModel[leg].QP_O = _dogModel[leg].OP_init - _dogModel[leg].OQ_now;
        _dogModel[leg].QP_Q = Rot.inverse() * _dogModel[leg].QP_O;
    }
}

template <typename T>
void SportsController<T>::calculatePosBias_ang(T roll, T pitch, T yaw) {
    roll = roll / 180.0 * 3.14159;
    pitch = pitch / 180.0 * 3.14159;
    yaw = yaw / 180.0 * 3.14159;
    calculatePosBias_rad(roll, pitch, yaw);
}

template <typename T>
void SportsController<T>::limitRpy(Vec3<T>& rpy, T max, T threshold, T alpha) {
    static Vec3<T> rpy_backup = rpy;

    for (int i = 0; i < 3; i++) {
        if (rpy[i] >= -threshold && rpy[i] <= threshold) {
            rpy[i] = 0;
        }
        if (fabs(rpy[i]) > max) {
            if (rpy[i] > 0)
                rpy[i] = max;
            else
                rpy[i] = -max;
        }
    }

    rpy = alpha * rpy + (1 - alpha) * rpy_backup;
    rpy_backup = rpy;
    // print
    // cout<<rpy<<endl;
}

template <typename T>
Vec3<T> SportsController<T>::limit(Vec3<T> in, Vec3<T> min, Vec3<T> max) {
    Vec3<T> res;
    for (int i = 0; i < 3; i++) {
        if (in(i) < min(i))
            res(i) = min(i);
        else if (in(i) > max(i))
            res(i) = max(i);
        else
            res(i) = in(i);
    }
    return res;
}
